#ifndef __GLOBAL_ROUTING_H
#define __GLOBAL_ROUTING_H

#include <iostream>
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include "std_msgs/String.h"
#include "ReferenceLine_Split.h"
#include "global_path_struct.h"
#include "stanley_controller.h"
#include "pure_pursuit_controller.h"
#include "mpc_controller.h"
#include "lqr_controller.h"
#include "pid_controller.h"
#include <visualization_msgs/MarkerArray.h>
#include "carla_msgs/CarlaEgoVehicleControl.h"
#include "ackermann_msgs/AckermannDrive.h"
#include <waypoint_msgs/Waypoint.h>
#include <waypoint_msgs/WaypointArray.h>
#include "controller.h"

using namespace std;
using namespace carla_pnc;

class GlobalRouting
{
public:
  GlobalRouting();
  ~GlobalRouting();
  void thread_routing(void);

  void odom_call_back(const nav_msgs::Odometry &msg);
  void start_pose_call_back(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
  void goal_pose_call_back(const geometry_msgs::PoseStamped::ConstPtr &msg);
  void get_Waypoints_From_Carla(const nav_msgs::Path &waypoints);

  void Vehival_Theta(const geometry_msgs::PoseArray &theta);
  void Vehival_Kappa(const geometry_msgs::PoseArray &kappa);
  void Vehival_Go(const std_msgs::String::ConstPtr &go);
  void Vehival_Traj(const waypoint_msgs::WaypointArray::ConstPtr &msg);
  bool Vehical_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &odom);

  void create_map();
  void publish_car_start_pose(const geometry_msgs::Pose &start_pose);
  void CreateLqrOffline(const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R);
  Eigen::MatrixXd calc_dlqr(double vx, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R);

  /***********************************整车参数**************************************/
  double L = 3.0;                                                         // 轴距
  double cf = -155494.663;                                                // 前轮侧偏刚度,左右轮之和
  double cr = -155494.663;                                                // 后轮侧偏刚度, 左右轮之和
  double mass_fl = 1845.0 / 4;                                            // 左前悬的质量
  double mass_fr = 1845.0 / 4;                                            // 右前悬的质量
  double mass_rl = 1845.0 / 4;                                            // 左后悬的质量
  double mass_rr = 1845.0 / 4;                                            // 右后悬的质量
  double mass_front = mass_fl + mass_fr;                                  // 前悬质量
  double mass_rear = mass_rl + mass_rr;                                   // 后悬质量
  double mass = mass_front + mass_rear;                                   // 车辆载荷
  double lf = L * (1.0 - mass_front / mass);                              // 汽车前轮到中心点的距离
  double lr = L * (1.0 - mass_rear / mass);                               // 汽车后轮到中心点的距离
  double Iz = std::pow(lf, 2) * mass_front + std::pow(lr, 2) * mass_rear; // 车辆绕z轴转动的转动惯量
protected:
  std::vector<Eigen::MatrixXd> lqr_k_table;           // LQR离线求解后的k
  double k_pure;                                      // PurePursuit"增益系数
  double k_cte;                                       // Stanley"增益系数
  double Q_ed, Q_ed_dot, Q_ephi, Q_ephi_dot, R_value; // LQR_dynamics Q  R矩阵权重
  double Q_ex_k, Q_ed_k, Q_ephi_k, R_value_k;         // LQR_kinematics Q  R矩阵权重
  double kp, ki, kd;                                  // 纵向PID

private:
  Eigen::MatrixXd hdmap_way_points;           // 中心点坐标
  std::vector<double> speeds, thetas, kappas; // 获取局部规划的速度与航向角,曲率
  std::vector<car_state> local_waypoints;     // 局部规划路径点
  nav_msgs::Odometry car_odom_;
  car_state cur_pose; // 车辆当前状态

  // 函数对象
  ReferenceLine_Split Rs;                   // 参考线生成
  geometry_msgs::Vector3 msg_ros;           // 发布控制数据
  ackermann_msgs::AckermannDrive msg_carla; // 发布控制数据
  visualization_msgs::MarkerArray obstacle_points, map_points;

  // 控制算法
  StanleyController Stanly;
  PursuitController Pursuit;
  LqrController Lqr;
  PidController Pid;
  MpcController Mpc;

  // 路径点
  nav_msgs::Path dynamic_points;
  Eigen::MatrixXd hdmap_way_points_;

  // 点
  geometry_msgs::Pose start_pose_;
  geometry_msgs::Pose goal_pose_;

  // visual Pub
  ros::Publisher obstacle_points_pub_;
  ros::Publisher goal_point_pub_;
  ros::Publisher map_points_pub_;
  ros::Publisher control_data_pub_;
  ros::Publisher vehicle_start_pose_pub_;
  ros::Publisher control_cmd_pub_; // 发布carla控制命令
  // visual Subscriber
  ros::Subscriber odom_sub_;
  ros::Subscriber waypoints_sub_;
  ros::Subscriber start_pose_subscriber_;
  ros::Subscriber goal_pose_subscriber_;
  ros::Subscriber vehival_theta;
  ros::Subscriber vehival_kappa;
  ros::Subscriber vehival_go;
  ros::Subscriber vehival_traj;
  // thread
  boost::thread *routing_thread_;
  // flag
  bool carla_simulation;   //
  bool parking_simulation; //
  string role_name, control_method;
  bool is_begin_reference;  // 控制参考线发布一次
  int obstacle_id;          // 手动标注障碍物位置坐标显示
  double car_speed;         // 车速度
  double goal_distanse;     // 车速度
  bool is_vehical_stop_set; // 判断车是否要停下来
  bool is_start_pose_set;   // 是否定义了起点
  bool is_goal_pose_set;    // 是否定义了终点
  int use_what_planner;     // 规划算法
  bool is_open_scense;      //判别是否是开放空间的规划算法
  int use_what_controller;  // 控制算法
  string Start_dynamic;     // 判断局部规划是否开始
};

#endif